 #include "WPILib.h"
#include "Constants.h"
#include "TargetingCamera.h"
#include <cmath>
#include <cstdlib>


TargetingCamera::TargetingCamera()
{	
	dashboard = SmartDashboard::GetInstance();
	team830Table = NetworkTable::GetTable("Team830");
}

bool TargetingCamera::FindTarget(float &distance)
{
	if (!team830Table->IsConnected() || !team830Table->GetBoolean("freshImage"))
	{
		return false;
	}
	
	float tlx = atof(team830Table->GetString("tlx").c_str());
	float trx = atof(team830Table->GetString("trx").c_str());
	float blx = atof(team830Table->GetString("blx").c_str());
	float brx = atof(team830Table->GetString("brx").c_str());
	
	// Escape clause if no target is found.
	if (tlx < 0.0 || trx < 0.0 || blx < 0.0 || brx < 0.0) 
	{
		return false;
	}
	
	float lx = (tlx + blx) * 0.5;
	float rx = (trx + brx) * 0.5;
	float feetPerPixel = WIDTH_OF_TARGET_FEET / (lx - rx);
	float fieldOfViewWidth = feetPerPixel * IMAGE_WIDTH;
	
	float skewTemp = (tlx - trx) * 0.5 * feetPerPixel;
	float skewOffset = sqrt(0.25 * WIDTH_OF_TARGET_FEET * WIDTH_OF_TARGET_FEET - skewTemp * skewTemp);
	
	
	float shotLength = (fieldOfViewWidth * 0.5) / tan(CAMERA_FOV * 0.5);
	double temp = shotLength * shotLength - 6.25 * 6.25;
	if (temp <= 0.0) return false; //OH CRAP
	distance = sqrt(temp);
	distance += 1.5;
	distance -= skewOffset;
	//Escape clause. Distance should never be over 25 feet.
	if (distance > 25.0) return false;
<<<<<<< .mine
=======

>>>>>>> .r122
	
	return true;
}




